#include "gd32f1x0_libopt.h"
#include "i2c.h"
#include "systick.h"
#include "fusb302.h"
#include "gpio.h"
#include "tusb564.h"

#define BL_ADJ_VAL_NUM_MAX 10
#define waitPDMessageTimeMax 100

#define BL_PWM_DEFAULT 199

volatile bool finaly = FALSE;
volatile bool finish = FALSE;
bool in_bl_mode = TRUE;
volatile bool turn_on_bl = FALSE;
volatile bool connect = FALSE;
volatile uint16_t BL_ADJ_VAL[BL_ADJ_VAL_NUM_MAX] = {0};
uint8_t BL_ADJ_VAL_NUM = 0;
uint16_t dB_adj = 0;
volatile bool noPDMessage = TRUE;
volatile uint16_t waitPDMessageTime = 0;

#define FUSB302_Int             GA11

#define HPD                     GB0

#define BL_EN                   GA7
#define BL_SWITCH               GA10

#define BL_PWM                  GA5

#define BL_ADJ                  GB1

#define MODE                    GA9

#define BASE_FLASH_ADDR         0x0800FC00
#define DP_EQ_ADDR              BASE_FLASH_ADDR + 0*4

void msg_recv() {
    if (!fusb302_is_rx_empty()) {

        uint16_t  usb_pd_message_header;
        uint32_t  usb_pd_message_buffer[10];
        uint8_t token = fusb302_get_message(&usb_pd_message_header, usb_pd_message_buffer);

        if (token) {
            // char out[50];
            // sprintf(out, "Token: %x\n", token);
            // USART_SendBytes(out);
            // sprintf(out, "Header: %x\n", usb_pd_message_header);
            // USART_SendBytes(out);
            // USART_SendBytes(", Header: ");
            // // USART_SendBytesln(HEADER_PARSE(usb_pd_message_header, usb_pd_message_buffer));
            uint8_t Obj_Num = PD_HEADER_CNT(usb_pd_message_header); // 报文数量
            if(Obj_Num) { // 数据报文
                uint8_t PDHeaderType = PD_HEADER_TYPE(usb_pd_message_header); // PD消息类型
                if ( PDHeaderType == 1) { // SOURCE_CAPABILITIES
                    uint8_t ObjId = 1;
                    uint16_t ReqVol = 100; // 报文中的电压 单位50mV
                    uint16_t ReqCur = 200; // 报文中的电流 单位10mA
                    uint16_t TypeVol = 240; // 12V在报文中对应的数值 单位50mV
                    for (uint8_t i = 0; i < Obj_Num; i++)
                    {
                        uint16_t Vol = (usb_pd_message_buffer[i] >> 10) & 0x3FF;
                        if (Vol <= TypeVol && Vol > ReqVol)
                        {
                            ReqVol = Vol;
                            ObjId = i + 1;
                        }
                    }
                    if (ReqVol == 240 ) {
                        ReqCur = 100;
                    }else if (ReqVol == 180) {
                        ReqCur = 150;
                    }
                    if(!fusb302_is_cc_busy()) {
                        // USART_SendBytes("send\n");
                        uint32_t msg = 0x03000000 | (ObjId << 28) | (ReqCur << 10) | ReqCur;
                        fusb302_send_message(0x1482, &msg, 0);
                    }else {
                        // USART_SendBytes("busy\n");
                    }
                }else if(PDHeaderType == 15) { // VENDOR_DEFINED
                    uint32_t vdm_header = usb_pd_message_buffer[0];
                    uint32_t header = 0x008F;
                    uint32_t data[10];
                    bool send = TRUE;
                    if(!fusb302_is_cc_busy()){
                        switch(vdm_header & 0x1F) {
                            case 1:
                                header |= 5 << 12;
                                data[0] = 0xFF00A041;
                                data[1] = 0x6C0004E8;
                                data[2] = 0xF4246;
                                data[3] = 0xA0201202;
                                data[4] = 0x110000DB;
                                break;
                            case 2:
                                header |= 3 << 12;
                                data[0] = 0xFF00A042;
                                data[1] = 0xFF0104E8;
                                data[2] = 0x0;
                                break;
                            case 3:
                                header |= 2 << 12;
                                data[0] = 0xFF01A043;
                                data[1] = 0x40045;
                                break;
                            case 4:
                                header |= 1 << 12;
                                data[0] = 0xFF01A144;
                                break;
                            case 16:
                                header |= 2 << 12;
                                data[0] = 0xFF01A150;
                                data[1] = 0xA;
                                break;
                            case 17:
                                header |= 1 << 12;
                                data[0] = 0xFF01A151;
                                finaly = TRUE;
                                break;
                            default:
                                send = FALSE;
                        }
                        if(send) {
                            fusb302_send_message(header, data, 0);
                        }
                        // USART_SendBytes("send\n");
                    }else {
                        // USART_SendBytes("busy\n");
                    }
                }
            }
        }
    }else {
        // USART_SendBytes("fusb302 is rx empty\n");
    }

}

void PDSend(uint16_t head, uint32_t* payload, uint8_t sop) {
    uint8_t time = 10;
    while (fusb302_is_cc_busy() && time --){
        delay_1ms(1);
    }
    if(time){
        fusb302_send_message(head, payload, sop);
    }
}

void PDSendHPD(bool state) {
    uint32_t data[] = {0xFF01A106, 0x8A};
    if (!state){
        data[1] = 0xA;
    }
    PDSend(0x208F, data, 0);
}

void turnOnBL() {
    if(!turn_on_bl) {
        gpio_write(BL_EN, 1);
        turn_on_bl = TRUE;
        if (finish){
            PDSendHPD(TRUE);
        }
    }
}

void turnOffBL() {
    if(turn_on_bl) {
        gpio_write(BL_EN, 0);
        turn_on_bl = FALSE;
        if (finish){
            PDSendHPD(FALSE);
        }
    }
}

void vbusOk() {
    fusb302_ufp_auto_polarity();
    if(fusb302_is_connect_cc1()){
        tusb564_FLIP(FALSE);
        connect = TRUE;
    }else if(fusb302_is_connect_cc2()){
        tusb564_FLIP(TRUE);
        connect = TRUE;
    }else {
        connect = FALSE;
    }
    if (connect) {
        turnOnBL();
    }else {
        finish = FALSE;
        noPDMessage = TRUE;
    }
}

void interrupt_handler() {
    uint32_t reason = fusb302_get_interrupt_reason();
    while (reason != 0)
    {
        if (reason & FUSB302_INT_BC_LVL) {
            // USART_SendBytes("FUSB302_INT_BC_LVL\n");
        }
        if (reason & FUSB302_INT_COLLISION) {
            // USART_SendBytes("FUSB302_INT_COLLISION\n");
        }
        if (reason & FUSB302_INT_WAKE) {
            // USART_SendBytes("FUSB302_INT_WAKE\n");
        }
        if (reason & FUSB302_INT_ALERT) {
            // USART_SendBytes("FUSB302_INT_ALERT\n");
        }
        if (reason & FUSB302_INT_CRC_CHK) {
            USART_SendBytes("FUSB302_INT_CRC_CHK\n");
            msg_recv();
            noPDMessage = FALSE;
            waitPDMessageTime = 0;
        }
        if (reason & FUSB302_INT_COMP_CHNG) {
            // USART_SendBytes("FUSB302_INT_COMP_CHNG\n");
        }
        if (reason & FUSB302_INT_ACTIVITY) {
            // USART_SendBytes("FUSB302_INT_ACTIVITY\n");
            // msg_recv();
        }
        if (reason & FUSB302_INT_VBUSOK) {
            USART_SendBytes("FUSB302_INT_VBUSOK\n");
            vbusOk();
        }
        if (reason & FUSB302_INT_HARDRESET) {
            // USART_SendBytes("FUSB302_INT_HARDRESET\n");
        }
        if (reason & FUSB302_INT_SOFTRESET) {
            // USART_SendBytes("FUSB302_INT_SOFTRESET\n");
        }
        if (reason & FUSB302_INT_TX_SUCCESS) {
            // USART_SendBytes("FUSB302_INT_TX_SUCCESS\n");
        }
        if (reason & FUSB302_INT_HARDSENT) {
            // USART_SendBytes("FUSB302_INT_HARDSENT\n");
        }
        if (reason & FUSB302_INT_RETRYFAIL) {
            // USART_SendBytes("FUSB302_INT_RETRYFAIL\n");
        }
        if (reason & FUSB302_INT_SOFTFAIL) {
            // USART_SendBytes("FUSB302_INT_SOFTFAIL\n");
        }
        if (reason & FUSB302_INT_TOGDONE) {
            // USART_SendBytes("FUSB302_INT_TOGDONE\n");
        }
        if (reason & FUSB302_INT_OCP_TEMP) {
            // USART_SendBytes("FUSB302_INT_OCP_TEMP\n");
        }
        if (reason & FUSB302_INT_GCRCSENT) {
            // USART_SendBytes("FUSB302_INT_GCRCSENT\n");
        }
        reason = fusb302_get_interrupt_reason();
    }
}

void timer_set(void) {
    timer_oc_parameter_struct timer_ocintpara;
    timer_parameter_struct timer_initpara;

    rcu_periph_clock_enable(RCU_TIMER1);

    timer_deinit(TIMER1);

    /* TIMER1 configuration */
    timer_initpara.prescaler         = 71;
    timer_initpara.alignedmode       = TIMER_COUNTER_EDGE;
    timer_initpara.counterdirection  = TIMER_COUNTER_UP;
    timer_initpara.period            = 2499;
    timer_initpara.clockdivision     = TIMER_CKDIV_DIV1;
    timer_initpara.repetitioncounter = 0;
    timer_init(TIMER1,&timer_initpara);

    /* CH1, CH2 and CH3 configuration in PWM0 mode */
    timer_ocintpara.ocpolarity  = TIMER_OC_POLARITY_HIGH;
    timer_ocintpara.outputstate = TIMER_CCX_ENABLE;

    timer_channel_output_config(TIMER1, TIMER_CH_0, &timer_ocintpara);

    timer_channel_output_pulse_value_config(TIMER1, TIMER_CH_0, BL_PWM_DEFAULT);
    timer_channel_output_mode_config(TIMER1, TIMER_CH_0, TIMER_OC_MODE_PWM0);
    timer_channel_output_shadow_config(TIMER1, TIMER_CH_0, TIMER_OC_SHADOW_DISABLE);

    /* auto-reload preload enable */
    timer_auto_reload_shadow_enable(TIMER1);
    
    timer_enable(TIMER1);
}

void adc_config(void)
{
    rcu_periph_clock_enable(RCU_ADC);
    rcu_adc_clock_config(RCU_ADCCK_APB2_DIV6);
    adc_special_function_config(ADC_CONTINUOUS_MODE, ENABLE);
    /* ADC触发方式 */
    adc_external_trigger_source_config(ADC_REGULAR_CHANNEL, ADC_EXTTRIG_REGULAR_NONE); 
    /* ADC数据对齐方式 */
    adc_data_alignment_config(ADC_DATAALIGN_RIGHT);
    /* 使用一个ADC通道*/
    adc_channel_length_config(ADC_REGULAR_CHANNEL, 1);
    /* 设置ADC通道和转换时间 */
    adc_regular_channel_config(0, ADC_CHANNEL_9, ADC_SAMPLETIME_239POINT5);
    /* ADC外部触发配置 */
    adc_external_trigger_config(ADC_REGULAR_CHANNEL, ENABLE);

    /* 使能ADC */
    adc_enable();
    delay_1ms(1);
    /* 进行ADC自校准 */
    adc_calibration_enable();
}

void nvic_config(void)
{
    nvic_priority_group_set(NVIC_PRIGROUP_PRE2_SUB2);
    nvic_irq_enable(ADC_CMP_IRQn, 3, 2);
    nvic_irq_enable(EXTI4_15_IRQn, 3, 3);
    // FUSB302_Int
    syscfg_exti_line_config(EXTI_SOURCE_GPIOA, EXTI_SOURCE_PIN11);
    exti_init(EXTI_11, EXTI_INTERRUPT, EXTI_TRIG_FALLING);
    exti_interrupt_flag_clear(EXTI_11);
}

void EXTI4_15_IRQHandler(void)
{
    // USART_SendBytes("EXTI4_15_IRQHandler\n");
    if(RESET != exti_interrupt_flag_get(EXTI_11)){
        // USART_SendBytes("EXTI_11 Start\n");
        interrupt_handler();
        exti_interrupt_flag_clear(EXTI_11);
        // USART_SendBytes("EXTI_11 Finish\n\n");
    }
}

uint16_t  get_BL_ADJ_VAL() {
    /* 使能ADC软触发 */
    adc_software_trigger_enable(ADC_REGULAR_CHANNEL);
    /* 等待ADC转换完成 */
    while(!adc_flag_get(ADC_FLAG_EOC));
    /* 清除转换完成标志位 */
    adc_flag_clear(ADC_FLAG_EOC);
    /* 返回ADC的值 */
    return (adc_regular_data_read() & 0xfff);
}

dp_lane_dB get_dp_dB(uint32_t num){
    switch (num)
    {
    case 0: return dp_dB_F_0_3;
    case 1: return dp_dB_1_6;
    case 2: return dp_dB_3_0;
    case 3: return dp_dB_4_4;
    case 4: return dp_dB_5_4;
    case 5: return dp_dB_6_5;
    case 6: return dp_dB_7_3;
    case 7: return dp_dB_8_1;
    case 8: return dp_dB_8_9;
    case 9: return dp_dB_9_5;
    case 10: return dp_dB_10_0;
    case 11: return dp_dB_10_6;
    case 12: return dp_dB_11_0;
    case 13: return dp_dB_11_4;
    case 14: return dp_dB_11_8;
    case 15: return dp_dB_12_1;
    default:
        return dp_dB_5_4;
    }
}

bool save_dp_dB(uint32_t num) {
    uint32_t *ptr = (uint32_t *)DP_EQ_ADDR;
    fmc_unlock();
    fmc_flag_clear(FMC_FLAG_END | FMC_FLAG_WPERR | FMC_FLAG_PGERR);
    fmc_page_erase(BASE_FLASH_ADDR);
    if(0xFFFFFFFF == (*ptr)){
        fmc_word_program(DP_EQ_ADDR, num);
        if(num == (*ptr)){
            fmc_flag_clear(FMC_FLAG_END | FMC_FLAG_WPERR | FMC_FLAG_PGERR);
            fmc_lock();
            return TRUE;
        }
    }
    return FALSE;
}

void gpio_set(void) {

    gpio_enable(FUSB302_Int, GPIO_MODE_INPUT);

    gpio_enable(HPD, GPIO_MODE_INPUT);
    gpio_enable_pull(BL_SWITCH, GPIO_MODE_INPUT, GPIO_PUPD_PULLUP);

    gpio_enable(BL_EN, GPIO_MODE_OUTPUT);

    gpio_enable_AF(BL_PWM, GPIO_AF_2);

    gpio_enable(BL_ADJ, GPIO_MODE_ANALOG);

    gpio_enable_pull(MODE, GPIO_MODE_INPUT, GPIO_PUPD_PULLUP);
}

void setup() {

    systick_config();

    gpio_set();
    nvic_config();
    timer_set();
    adc_config();
    
    IIC_Init();
    // USART_Init();

    // if(IIC_CheckDevice(TUSB564_I2C_SLAVE_ADDR)){
    //     USART_SendBytes("cant find TUSB564\n");
    // }else{
    //     USART_SendBytes("find TUSB564\n");
    // }

    // if(IIC_CheckDevice(FUSB302_I2C_SLAVE_ADDR)){
    //     USART_SendBytes("cant find FUSB302\n");
    // }else{
    //     USART_SendBytes("find FUSB302\n");
    // }

    fusb302_reset();

    fusb302_autocrc_config(TRUE, FALSE, 1, FALSE);
    //Configure UFP:
    fusb302_cc_pu_switch(FALSE, FALSE);
    //fusb302_cc_pu_current(3);
    fusb302_cc_pd_switch(TRUE, TRUE);

    fusb302_cc_vconn_switch(FALSE, FALSE);
    fusb302_cc_meas_switch(FALSE, FALSE);
    fusb302_vbus_meas_en(FALSE);

    fusb302_cc_bmc_en(FALSE, FALSE);
    fusb302_sop_prime_en(TRUE);

    fusb302_mdac_set(0x30); //2.05V

    fusb302_get_interrupt_reason();//Clear INT
    fusb302_int_en(TRUE);
    fusb302_set_interrupt_mask(FUSB302_INT_CRC_CHK | FUSB302_INT_VBUSOK);
    fusb302_get_interrupt_reason();//Clear INT

    fusb302_tcpc_write(TCPC_REG_POWER, TCPC_REG_POWER_PWR_ALL);

    fusb302_tcpc_write(TCPC_REG_RESET, TCPC_REG_RESET_PD_RESET);

    tusb564_EQ(FALSE);
    tusb564_DP_EN(TRUE);
    tusb564_CTLSEL(DP_Only);

    uint32_t DP_EQ = *(uint32_t *)DP_EQ_ADDR;
    if(DP_EQ != 0xFFFFFFFF) {
        dp_lane_dB dp_dB = get_dp_dB(DP_EQ);
        tusb564_DP0EQ(dp_dB);
        tusb564_DP1EQ(dp_dB);
        tusb564_DP2EQ(dp_dB);
        tusb564_DP3EQ(dp_dB);
    }else {
        tusb564_DP0EQ(dp_dB_5_4);
        tusb564_DP1EQ(dp_dB_5_4);
        tusb564_DP2EQ(dp_dB_5_4);
        tusb564_DP3EQ(dp_dB_5_4);
    }
    
    vbusOk();
}

void loop() {
    if(connect) {
        if(noPDMessage) {
            if(waitPDMessageTime < waitPDMessageTimeMax){
                waitPDMessageTime++;
            }
            if (waitPDMessageTime >= waitPDMessageTimeMax) {
                waitPDMessageTime = 0;
                fusb302_cc_pd_switch(FALSE, FALSE);
                delay_1ms(800);
                fusb302_cc_pd_switch(TRUE, TRUE);
            }
        }
        if (finaly) {
            if (gpio_read(HPD))
            {
                // USART_SendBytes("HPD OK\n");
                finaly = FALSE;
                finish = TRUE;
                PDSendHPD(TRUE);
            }
            
        }
        if(finish) {
            if(gpio_read(MODE)){
                in_bl_mode = TRUE;
            }else {
                in_bl_mode = FALSE;
            }

            if (gpio_read(BL_SWITCH)){
                turnOnBL();
            }else {
                turnOffBL();
            }

            if(in_bl_mode) {
                uint16_t adjVal = get_BL_ADJ_VAL();
                if (adjVal != 0) {
                    BL_ADJ_VAL[BL_ADJ_VAL_NUM++] = adjVal;
                    if (BL_ADJ_VAL_NUM == BL_ADJ_VAL_NUM_MAX)
                        BL_ADJ_VAL_NUM = 0;
                    uint32_t BL_ADJ_Aver = 0;
                    for (uint8_t i = 0; i < BL_ADJ_VAL_NUM_MAX; i++){
                        BL_ADJ_Aver += BL_ADJ_VAL[i];
                    }
                    BL_ADJ_Aver = BL_ADJ_Aver / BL_ADJ_VAL_NUM_MAX;
                    uint32_t pulse_value = BL_ADJ_Aver * 2300 / 4095 + BL_PWM_DEFAULT;
                    timer_channel_output_pulse_value_config(TIMER1, TIMER_CH_0, pulse_value);
                }
            }else {
                dB_adj = get_BL_ADJ_VAL() / 256;
                dp_lane_dB dp_dB = get_dp_dB(dB_adj);
                tusb564_DP0EQ(dp_dB);
                tusb564_DP1EQ(dp_dB);
                tusb564_DP2EQ(dp_dB);
                tusb564_DP3EQ(dp_dB);
                save_dp_dB(dB_adj);
            }
            delay_1ms(50);
        }
        
    }else {
        for (uint8_t i = 0; i < BL_ADJ_VAL_NUM_MAX; i++) {
            BL_ADJ_VAL[i] == 0;
        }
        timer_channel_output_pulse_value_config(TIMER1, TIMER_CH_0, BL_PWM_DEFAULT);
        turnOffBL();
    }
    delay_1ms(1);
}

int main(void)
{
    setup();
    while(1){
        loop();
    }
}
